import os
import re
import time
from datetime import datetime

import numpy as np
from PyQt5 import QtGui
from PyQt5.QtCore import QThread, pyqtSignal, Qt
from PyQt5.QtWidgets import QMainWindow, QFileDialog
import yaml

# import BaseFunctions_INS as ins
# import LooselyCoupleCore as LC
from config import Config
from loosely_couple_core import Option, post_loosely_couple
from ui_loose_couple import Ui_LooseCoupleWindow


class LooseCoupleThread(QThread):
    progress_signal = pyqtSignal(int)
    log_signal = pyqtSignal(str)

    def __init__(self, opt: Option, engine=0):
        super(LooseCoupleThread, self).__init__()
        self.opt = opt
        self.engine = engine

    def send2log(self, msg: str):
        print(msg)
        self.log_signal.emit(msg)

    def run(self) -> None:
        opt = self.opt
        '''打开文件'''
        post_loosely_couple(opt, self.progress_signal)
        self.progress_signal.emit(100)


class PsinsThread(QThread):
    progress_signal = pyqtSignal(int)

    def __init__(self, yml_path: str):
        super(PsinsThread, self).__init__()
        self.yml_path = yml_path

    def run(self) -> None:
        # 调用psins处理
        os.system("exec/psins %s" % (self.yml_path))
        self.progress_signal.emit(100)


class LooseCoupleWindow(QMainWindow, Ui_LooseCoupleWindow):
    def __init__(self):
        super(LooseCoupleWindow, self).__init__()
        self.setupUi(self)
        self.btnBrowseImu.clicked.connect(self.onBtnBrowseImuFile)
        self.btnBrowseGps.clicked.connect(self.onBtnBrowseGNSSFile)
        self.btnBrowseCfg.clicked.connect(self.onBtnBrowseCfgFile)
        self.btnBrowseOdo.clicked.connect(self.onBtnBrowseOdoFile)
        self.btnBrowseOutput.clicked.connect(self.onBtnBrowseOutputFile)
        # self.btnBrowseRef.clicked.connect(self.onBtnBrowseRefFile)
        self.btnStart.clicked.connect(self.onBtnStart)
        self.btnClearLog.clicked.connect(self.clear_log)

        self.tem_yml_path = "yaml/configure.yml"
        # if os.path.isfile(self.tem_yml_path):
        # os.makedirs(self.tem_yml_path)
        self.options = {}
        self.yml = {}
        self.get_ui()

        keys = [
            "imu-path", "imu-parameter-cfg", "gnss-path", "output-path",
            "start-time",
            "end-time",
            "imu-data-rate",
            "alignment-mode",
            "alignment-epoch",
            "init-pos-std",
            "init-vel-std",
            "init-std-std",
            "alignment-moving-vel",
            "gnss-format",
            "gnss-data-rate",
            "antenna-level-arm",
            "odo-enable",
            "odo-path"
            "odo-wheel-radius",
            "odo-level-arm",
        ]
        self.keys = keys
        self.__load_cfg()

    def get_ui(self):
        self.options["a"] = 0

    def __get_last_line(self, filename):
        """
        get last line of a file
        :param filename: file name
        :return: last line or None for empty file
        """
        try:
            filesize = os.path.getsize(filename)
            print(filesize)
            if filesize == 0:
                return None
            else:
                with open(filename, 'rb') as fp:  # to use seek from end, must use mode 'rb'
                    offset = -8  # initialize offset
                    while -offset < filesize:  # offset cannot exceed file size
                        fp.seek(offset, 2)  # read # offset chars from eof(represent by number '2')
                        lines = fp.readlines()  # read from fp to eof
                        if len(lines) >= 2:  # if contains at least 2 lines
                            return lines[-1]  # then last line is totally included
                        else:
                            offset *= 2  # enlarge offset
                    fp.seek(0)
                    lines = fp.readlines()
                    return lines[-1]
        except FileNotFoundError:
            print(filename + ' not found!')
            return None

    def onBtnBrowseImuFile(self):
        filfilepath, suffix = QFileDialog.getOpenFileName(filter=' IMU files (*.imutxt);;All Files (*)')
        self.editImuPath.setText(filfilepath)
        self.show_log("imupath: %s" % filfilepath)

        ## 显示开始时间
        if os.path.isfile(filfilepath):
            try:
                with open(filfilepath, "r") as f:
                    first_line = f.readline()
                    start_t = int(np.ceil(float(first_line.split()[0])))
                    print(start_t)
                last_line = self.__get_last_line(filfilepath)
                end_t = int(np.ceil(float(last_line.decode(encoding="utf-8").split()[0])))
                print(end_t)
                # self.show_log("last line:\n"+last_line)
                # imudata = np.loadtxt(filfilepath)
                # self.show_log("row:%d column:%d" % (imudata.shape[0], imudata.shape[1]))
                # self.show_log("imu data start from %f to %f" % (imudata[0, 0], imudata[-1, 0]))
                self.treeWidget.topLevelItem(2).setText(1, str(start_t))
                self.treeWidget.topLevelItem(3).setText(1, str(end_t))
                self.editOutputPath.setText(filfilepath + ".loose_couple_python.nav")
            except Exception as e:
                self.show_log(e.args[0])
                return

    def onBtnBrowseGNSSFile(self):
        filfilepath, suffix = QFileDialog.getOpenFileName(filter=' GNSS files (*.gps);;All Files (*)')
        self.editGpsPath.setText(filfilepath)
        self.show_log("gnss path: %s" % filfilepath)
        ## 显示开始时间
        if os.path.isfile(filfilepath):
            try:
                data = np.loadtxt(filfilepath)
                self.show_log("row:%d column:%d" % (data.shape[0], data.shape[1]))
                self.show_log("imu data start from %f to %f" % (data[0, 0], data[-1, 0]))
                # self.treeWidget.topLevelItem(2).setText(1, str(data[0, 0]))
                # self.treeWidget.topLevelItem(3).setText(1, str(data[-1, 0]))
            except Exception as e:
                self.show_log(e.args[0])
                return

    def onBtnBrowseCfgFile(self):
        filfilepath, suffix = QFileDialog.getOpenFileName(filter=' Configure files (*.yml);;All Files (*)')
        self.editCfgPath.setText(filfilepath)
        self.show_log("cfgpath: %s" % filfilepath)

    def onBtnBrowseOdoFile(self):
        filfilepath, suffix = QFileDialog.getOpenFileName(filter=' Odometer files (*.odotxt);;All Files (*)')
        self.editOdoPath.setText(filfilepath)
        self.show_log("odopath: %s" % filfilepath)

    def onBtnBrowseOutputFile(self):
        filfilepath, suffix = QFileDialog.getSaveFileName(filter=' Nav files (*.nav);;All Files (*)')
        if suffix == "":
            return
        if not re.match('.*.nav', filfilepath):
            filfilepath += '.nav'
        self.editOutputPath.setText(filfilepath)
        self.show_log("outputpath: %s" % filfilepath)

    def onBtnBrowseRefFile(self):
        filfilepath, suffix = QFileDialog.getOpenFileName(filter=' Reference files (*.nav);;All Files (*)')
        self.editRefPath.setText(filfilepath)
        self.show_log("imupath: %s" % filfilepath)

    def check_option(self, yml: dict):
        checked = True
        if not os.path.isfile(yml["imu-path"]):
            checked = False
            self.show_log("imu file not exit")

        if not os.path.isfile(yml["gnss-path"]):
            checked = False
            self.show_log("gps file not exit")

        if not os.path.isfile(yml["imu-parameter-cfg"]):
            checked = False
            self.show_log("cfg file not exit %s" % yml["imu-parameter-cfg"])

        if yml["start-time"] - yml["end-time"] >= 0:
            checked = False
            self.show_log("end time(%d) < start time(%d) " % (yml["start-time"], yml["end-time"]))

        if yml["alignment-mode"] == 0:
            if yml["alignment-epoch"][1] < yml["start-time"]:
                checked = False
                self.show_log("given initial state must past start time %d" % yml["start-time"])
        return checked

    def __load_cfg(self):
        self.show_log("load configure from %s" % self.tem_yml_path)
        keys = self.keys
        try:
            with open(self.tem_yml_path, 'r') as f:
                yml = yaml.safe_load(f)
                print(yml)
                self.editImuPath.setText(yml[keys[0]])
                self.editCfgPath.setText(yml[keys[1]])
                self.editOutputPath.setText(yml[keys[3]])
                # self.editOdoPath.setText(yml[keys[18]])
                self.editGpsPath.setText(yml[keys[2]])
                self.treeWidget.topLevelItem(0).setText(1, str(yml["start-time"]))
                self.treeWidget.topLevelItem(1).setText(1, str(yml["end-time"]))
                self.treeWidget.topLevelItem(2).setText(1, str(yml["imu-data-rate"]))
                self.treeWidget.topLevelItem(3).setText(1, str(yml["gnss-format"]))
                self.treeWidget.topLevelItem(4).setText(1, str(yml["gnss-data-rate"]))
                self.treeWidget.topLevelItem(5).setText(1, str(yml["antenna-level-arm"]))
                self.treeWidget.topLevelItem(5).child(0).setText(1, str(yml["antenna-level-arm"][0]))
                self.treeWidget.topLevelItem(5).child(1).setText(1, str(yml["antenna-level-arm"][1]))
                self.treeWidget.topLevelItem(5).child(2).setText(1, str(yml["antenna-level-arm"][2]))
                self.treeWidget.topLevelItem(6).setText(1, str(yml["alignment-mode"]))
                for i in range(11):
                    self.treeWidget.topLevelItem(7).child(i).setText(1, str(yml["alignment-epoch"][i]))
                for i in range(3):
                    self.treeWidget.topLevelItem(8).child(i).setText(1, str(yml["init-pos-std"][i]))
                    self.treeWidget.topLevelItem(9).child(i).setText(1, str(yml["init-vel-std"][i]))
                    self.treeWidget.topLevelItem(10).child(i).setText(1, str(yml["init-vel-std"][i]))
                self.treeWidget.topLevelItem(11).setText(1, str(yml["alignment-moving-vel"]))
        except Exception as e:
            self.show_log(str(e.args[0]))
        return

    def __save_cfg(self):
        keys = self.keys
        yml = {}
        try:
            yml[keys[0]] = self.editImuPath.text()
            yml[keys[1]] = self.editCfgPath.text()
            yml[keys[2]] = self.editGpsPath.text()
            yml[keys[17]] = self.editOdoPath.text()
            yml[keys[3]] = self.editOutputPath.text()

            yml["start-time"] = int(self.treeWidget.topLevelItem(0).text(1))
            yml["end-time"] = int(self.treeWidget.topLevelItem(1).text(1))
            yml["imu-data-rate"] = int(self.treeWidget.topLevelItem(2).text(1))
            yml["gnss-format"] = int(self.treeWidget.topLevelItem(3).text(1))
            yml["gnss-data-rate"] = int(self.treeWidget.topLevelItem(4).text(1))
            yml["antenna-level-arm"] = [
                float(self.treeWidget.topLevelItem(5).child(i).text(1)) for i in range(3)
            ]
            yml["alignment-mode"] = int(self.treeWidget.topLevelItem(6).text(1))
            yml["alignment-epoch"] = [
                float(self.treeWidget.topLevelItem(7).child(i).text(1)) for i in range(11)
            ]

            yml["init-pos-std"] = [float(self.treeWidget.topLevelItem(8).child(i).text(1)) for i in range(3)]
            yml["init-vel-std"] = [float(self.treeWidget.topLevelItem(9).child(i).text(1)) for i in range(3)]
            yml["init-atti-std"] = [float(self.treeWidget.topLevelItem(10).child(i).text(1)) for i in range(3)]
            yml["alignment-moving-vel"] = float(self.treeWidget.topLevelItem(11).text(1))
        except Exception as e:
            self.show_log(str(e.args[0]))
            return False
        if self.check_option(yml):
            try:
                with open(self.tem_yml_path, 'w') as f:
                    res = yaml.dump(yml, f)
                    print(res)
                    return True
            except Exception as e:
                self.show_log(str(e.args[0]))
                return False
        else:
            return False
        # self.yml = yml
        # 检查选项是否合理

    def onBtnStart(self):
        self.show_log("start loosely couple...")
        if not self.__save_cfg():
            return

        engine = self.cbxEngine.currentIndex()
        if engine == 0:
            '''python 版本'''
            self.progressBar.setEnabled(True)
            self.progressBar.setVisible(True)
            opt = Option(self.tem_yml_path)
            self.loose_couple_thread = LooseCoupleThread(opt, 0)
            self.loose_couple_thread.progress_signal.connect(self.onProgress)
            self.loose_couple_thread.log_signal.connect(self.show_log)
            self.loose_couple_thread.start()
            self.btnStart.setEnabled(False)
        elif engine == 1:
            self.loose_couple_thread = PsinsThread(self.tem_yml_path)
            self.loose_couple_thread.progress_signal.connect(self.onProgress)
            self.loose_couple_thread.start()
            self.btnStart.setEnabled(False)
        else:
            self.show_log("no such engine")

    def onProgress(self, progress):
        self.progressBar.setValue(progress)
        if progress == 100:
            self.btnStart.setEnabled(True)

    def show_log(self, msg):
        stamp = datetime.now().strftime('%H:%M:%S : ')
        item = stamp + msg
        self.listLog.addItem(item)

    def clear_log(self):
        self.listLog.clear()

    def closeEvent(self, a0: QtGui.QCloseEvent) -> None:
        pass
